Save raw points

config/config_deskewing.json

@@ -4,6 +4,8 @@
     "save_ply": false,
     "save_points_lidar": false,
     "save_points_imu": true,
-    "ply_path": "/tmp/dump/deskewed_points"
+    "save_raw_points": false,
+    "ply_path": "/tmp/dump/deskewed_points",
+    "raw_ply_path": "/tmp/dump/raw_points"
   }
 }

config/config_odometry_fastlio2.json

@@ -5,16 +5,16 @@
     "num_threads": 4,
     "max_iterations": 4,
     // Downsampling
-    "filter_size_surf": 0.5,
-    "filter_size_map": 0.5,
+    "filter_size_surf": 0.25,
+    "filter_size_map": 0.25,
     // Local map management
     "cube_side_length": 1000.0,
     "det_range": 300.0,
-    // IMU noise params
-    "gyr_cov": 0.1,
-    "acc_cov": 0.1,
-    "b_gyr_cov": 0.0001,
-    "b_acc_cov": 0.0001,
+    // IMU noise params (noise settings in config_sensors.json are used)
+    // "gyr_cov": 0.1,
+    // "acc_cov": 0.1,
+    // "b_gyr_cov": 0.0001,
+    // "b_acc_cov": 0.0001,
     // Online LiDAR-IMU extrinsic estimation
     "extrinsic_est_en": false
   }

modules/odometry/deskewing/include/glim_ext/deskewing_module.hpp

@@ -30,9 +30,11 @@ struct DeskewingParams {
 public:
   bool use_thread;  // Whether to use a separate thread for deskewing. If false, deskewing will be done in the odometry thread.
   bool save_ply;
+  bool save_raw_points;
   bool save_points_lidar;
   bool save_points_imu;
   std::string ply_path;
+  std::string raw_ply_path;
 };
 
 struct DeskewingResult {

modules/odometry/deskewing/src/glim_ext/deskewing_module.cpp

@@ -18,9 +18,11 @@ DeskewingParams::DeskewingParams() {
   Config config(GlobalConfigExt::get_config_path("config_deskewing"));
   use_thread = config.param<bool>("deskewing", "use_thread", true);
   save_ply = config.param<bool>("deskewing", "save_ply", false);
+  save_raw_points = config.param<bool>("deskewing", "save_raw_points", false);
   save_points_lidar = config.param<bool>("deskewing", "save_points_lidar", false);
   save_points_imu = config.param<bool>("deskewing", "save_points_imu", false);
   ply_path = config.param<std::string>("deskewing", "ply_path", "/tmp/dump/deskewed_points");
+  raw_ply_path = config.param<std::string>("deskewing", "raw_ply_path", "/tmp/dump/raw_points");
 }
 
 DeskewingParams::~DeskewingParams() {}
@@ -32,6 +34,10 @@ DeskewingModule::DeskewingModule(const DeskewingParams& params, const std::strin
     logger->info("creating dst directory: {}", params.ply_path);
     std::filesystem::create_directories(params.ply_path);
   }
+  if (params.save_raw_points) {
+    logger->info("creating dst directory: {}", params.raw_ply_path);
+    std::filesystem::create_directories(params.raw_ply_path);
+  }
 
   deskewing.reset(new CloudDeskewing());
 
@@ -65,6 +71,7 @@ bool DeskewingModule::needs_wait() const {
 }
 
 void DeskewingModule::on_new_frame(const EstimationFrame::ConstPtr& frame) {
+  logger->debug("received new frame {} at time {}", frame->id, frame->stamp);
   if (!params.use_thread) {
     process_frame(frame);
     return;
@@ -87,7 +94,6 @@ void DeskewingModule::task() {
 void DeskewingModule::process_frame(const EstimationFrame::ConstPtr& frame) {
   if (frame->imu_rate_trajectory.size() == 0) {
     logger->warn("IMU rate trajectory is empty. Set save_imu_rate_trajectory=true in config_odometry_*.json");
-    return;
   }
 
   if (frame->raw_frame == nullptr) {
@@ -100,7 +106,7 @@ void DeskewingModule::process_frame(const EstimationFrame::ConstPtr& frame) {
     return;
   }
 
-  logger->debug("deskewing frame at time {}", frame->stamp);
+  logger->debug("deskewing frame {} at time {}", frame->id, frame->stamp);
   auto result = deskew_frame(frame);
   on_deskeweing_result(result);
 
@@ -113,8 +119,8 @@ DeskewingResult::Ptr DeskewingModule::deskew_frame(const EstimationFrame::ConstP
   const Eigen::Isometry3d T_lidar_imu = frame->T_lidar_imu;
 
   auto points = std::make_shared<gtsam_points::PointCloudCPU>();
-  points->add_times(raw_points->times);
   points->add_points(raw_points->points);
+  points->add_times(raw_points->times);
   if (!raw_points->intensities.empty()) {
     points->add_intensities(raw_points->intensities);
   }
@@ -137,22 +143,27 @@ DeskewingResult::Ptr DeskewingModule::deskew_frame(const EstimationFrame::ConstP
     time_indices[i] = static_cast<int>(time_table.size()) - 1;
   }
 
-  // Integrate IMU trajectory (t, x, y, z, qx, qy, qz, qw) x N
-  const Eigen::Matrix<double, 8, -1>& imu_traj = frame->imu_rate_trajectory;
-
-  std::vector<double> imu_times(imu_traj.cols());
-  std::vector<Eigen::Isometry3d> imu_poses(imu_traj.cols());
-  for (int i = 0; i < imu_traj.cols(); i++) {
-    imu_times[i] = imu_traj(0, i);
-    imu_poses[i] = Eigen::Isometry3d::Identity();
-    imu_poses[i].translation() = imu_traj.block<3, 1>(1, i);
-    imu_poses[i].linear() = Eigen::Quaterniond(imu_traj(7, i), imu_traj(4, i), imu_traj(5, i), imu_traj(6, i)).toRotationMatrix();
-  }
-
   auto result = std::make_shared<DeskewingResult>();
   result->frame = frame;
   result->raw_points = points;
-  result->deskewed_points_lidar = deskewing->deskew(T_lidar_imu.inverse(), imu_times, imu_poses, frame->stamp, points->times_storage, points->points_storage);
+
+  if (frame->imu_rate_trajectory.size() > 0) {
+    // Integrate IMU trajectory (t, x, y, z, qx, qy, qz, qw) x N
+    const Eigen::Matrix<double, 8, -1>& imu_traj = frame->imu_rate_trajectory;
+
+    std::vector<double> imu_times(imu_traj.cols());
+    std::vector<Eigen::Isometry3d> imu_poses(imu_traj.cols());
+    for (int i = 0; i < imu_traj.cols(); i++) {
+      imu_times[i] = imu_traj(0, i);
+      imu_poses[i] = Eigen::Isometry3d::Identity();
+      imu_poses[i].translation() = imu_traj.block<3, 1>(1, i);
+      imu_poses[i].linear() = Eigen::Quaterniond(imu_traj(7, i), imu_traj(4, i), imu_traj(5, i), imu_traj(6, i)).toRotationMatrix();
+    }
+
+    result->deskewed_points_lidar = deskewing->deskew(T_lidar_imu.inverse(), imu_times, imu_poses, frame->stamp, points->times_storage, points->points_storage);
+  } else {
+    result->deskewed_points_lidar.assign(points->points, points->points + points->size());
+  }
 
   result->deskewed_points_imu.resize(result->deskewed_points_lidar.size());
   const Eigen::Isometry3d T_imu_lidar = T_lidar_imu.inverse();
@@ -206,6 +217,31 @@ void DeskewingModule::save_deskewed_frame(const DeskewingResult::Ptr& result) {
     glk::save_ply_binary(filename, ply);
     logger->debug("saved deskewed IMU points to {}", filename);
   }
+
+  if (params.save_raw_points) {
+    ply.vertices.resize(raw_points->points.size());
+    for (int i = 0; i < raw_points->points.size(); i++) {
+      ply.vertices[i] = raw_points->points[i].cast<float>().head<3>();
+    }
+
+    if (!raw_points->times.empty()) {
+      std::vector<float> times_f(raw_points->times.size());
+      std::copy(raw_points->times.begin(), raw_points->times.end(), times_f.begin());
+
+      auto time_prop = std::make_shared<glk::PLYPropertyBuffer<float>>("time", times_f.data(), times_f.size());
+      ply.properties.push_back(time_prop);
+
+      std::vector<double> abs_times(raw_points->times.size());
+      std::transform(raw_points->times.begin(), raw_points->times.end(), abs_times.begin(), [result](double t) { return result->frame->stamp + t; });
+
+      auto abs_time_prop = std::make_shared<glk::PLYPropertyBuffer<double>>("abs_time", abs_times.data(), abs_times.size());
+      ply.properties.push_back(abs_time_prop);
+    }
+
+    const std::string filename = fmt::format("{}/raw_{:06d}.ply", params.raw_ply_path, result->frame->id);
+    glk::save_ply_binary(filename, ply);
+    logger->debug("saved raw points to {}", filename);
+  }
 }
 
 }  // namespace glim

modules/odometry/deskewing/src/glim_ext/deskewing_module_ros2.cpp

@@ -59,6 +59,10 @@ void DeskewingModule::publish_deskewed_frame(const DeskewingResult::Ptr& result)
   }
 
   // Scan-end points
+  if (result->frame->imu_rate_trajectory.size() == 0) {
+    logger->warn("IMU rate trajectory is empty. Set save_imu_rate_trajectory=true in config_odometry_*.json");
+    return;
+  }
   const auto& imu_rate_traj = result->frame->imu_rate_trajectory;
   const Eigen::Matrix<double, 8, 1> imu_begin = imu_rate_traj.col(0);
   const Eigen::Matrix<double, 8, 1> imu_end = imu_rate_traj.col(imu_rate_traj.cols() - 1);

modules/odometry/fastlio2/include/glim_ext/odometry_estimation_fastlio2.hpp

@@ -27,11 +27,6 @@ struct OdometryEstimationFastLIO2Params {
   double cube_side_length;
   double det_range;
 
-  double gyr_cov;
-  double acc_cov;
-  double b_gyr_cov;
-  double b_acc_cov;
-
   bool extrinsic_est_en;
 };
 

modules/odometry/fastlio2/src/glim_ext/odometry_estimation_fastlio2.cpp

@@ -229,6 +229,18 @@ struct OdometryEstimationFastLIO2::Impl {
     Config sensor_config(GlobalConfig::get_config_path("config_sensors"));
     T_lidar_imu = sensor_config.param<Eigen::Isometry3d>("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity());
 
+    const double imu_acc_std = sensor_config.param<double>("sensors", "imu_acc_noise", 0.05);
+    const double imu_gyr_std = sensor_config.param<double>("sensors", "imu_gyro_noise", 0.01);
+    const double imu_int_std = sensor_config.param<double>("sensors", "imu_int_noise", 0.001);
+    const double imu_bias_acc_std = sensor_config.param<double>("sensors", "imu_bias_noise_acc", 1e-5);
+    const double imu_bias_gyr_std = sensor_config.param<double>("sensors", "imu_bias_noise_gyro", 1e-6);
+
+    const double rate = 100.0;
+    const double acc_cov = imu_acc_std * imu_acc_std * rate;
+    const double gyr_cov = imu_gyr_std * imu_gyr_std * rate;
+    const double b_acc_cov = imu_bias_acc_std * imu_bias_acc_std * rate;
+    const double b_gyr_cov = imu_bias_gyr_std * imu_bias_gyr_std * rate;
+
     // T_lidar_imu transforms points from IMU frame to LiDAR frame
     // FAST-LIO2 uses Lidar_R_wrt_IMU and Lidar_T_wrt_IMU which is T_imu_lidar
     // i.e., the LiDAR pose expressed in the IMU frame
@@ -246,10 +258,10 @@ struct OdometryEstimationFastLIO2::Impl {
     mean_gyr = V3D::Zero();
     cov_acc = V3D(0.1, 0.1, 0.1);
     cov_gyr = V3D(0.1, 0.1, 0.1);
-    cov_acc_scale = V3D(p.acc_cov, p.acc_cov, p.acc_cov);
-    cov_gyr_scale = V3D(p.gyr_cov, p.gyr_cov, p.gyr_cov);
-    cov_bias_gyr = V3D(p.b_gyr_cov, p.b_gyr_cov, p.b_gyr_cov);
-    cov_bias_acc = V3D(p.b_acc_cov, p.b_acc_cov, p.b_acc_cov);
+    cov_acc_scale = V3D(acc_cov, acc_cov, acc_cov);
+    cov_gyr_scale = V3D(gyr_cov, gyr_cov, gyr_cov);
+    cov_bias_gyr = V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov);
+    cov_bias_acc = V3D(b_acc_cov, b_acc_cov, b_acc_cov);
     angvel_last = V3D::Zero();
     acc_s_last = V3D::Zero();
     first_lidar_time = 0.0;
@@ -702,11 +714,6 @@ OdometryEstimationFastLIO2Params::OdometryEstimationFastLIO2Params() {
   cube_side_length = config.param<double>("odometry_estimation", "cube_side_length", 1000.0);
   det_range = config.param<double>("odometry_estimation", "det_range", 300.0);
 
-  gyr_cov = config.param<double>("odometry_estimation", "gyr_cov", 0.1);
-  acc_cov = config.param<double>("odometry_estimation", "acc_cov", 0.1);
-  b_gyr_cov = config.param<double>("odometry_estimation", "b_gyr_cov", 0.0001);
-  b_acc_cov = config.param<double>("odometry_estimation", "b_acc_cov", 0.0001);
-
   extrinsic_est_en = config.param<bool>("odometry_estimation", "extrinsic_est_en", false);
 }