diff --git a/config/config_deskewing.json b/config/config_deskewing.json index ccb210e..0c466f9 100644 --- a/config/config_deskewing.json +++ b/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" } } \ No newline at end of file diff --git a/config/config_odometry_fastlio2.json b/config/config_odometry_fastlio2.json index 444a73b..2e53930 100644 --- a/config/config_odometry_fastlio2.json +++ b/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 } diff --git a/modules/odometry/deskewing/include/glim_ext/deskewing_module.hpp b/modules/odometry/deskewing/include/glim_ext/deskewing_module.hpp index e14e95c..85fd347 100644 --- a/modules/odometry/deskewing/include/glim_ext/deskewing_module.hpp +++ b/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 { diff --git a/modules/odometry/deskewing/src/glim_ext/deskewing_module.cpp b/modules/odometry/deskewing/src/glim_ext/deskewing_module.cpp index 88085c3..0ed6a61 100644 --- a/modules/odometry/deskewing/src/glim_ext/deskewing_module.cpp +++ b/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("deskewing", "use_thread", true); save_ply = config.param("deskewing", "save_ply", false); + save_raw_points = config.param("deskewing", "save_raw_points", false); save_points_lidar = config.param("deskewing", "save_points_lidar", false); save_points_imu = config.param("deskewing", "save_points_imu", false); ply_path = config.param("deskewing", "ply_path", "/tmp/dump/deskewed_points"); + raw_ply_path = config.param("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(); - 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(time_table.size()) - 1; } - // Integrate IMU trajectory (t, x, y, z, qx, qy, qz, qw) x N - const Eigen::Matrix& imu_traj = frame->imu_rate_trajectory; - - std::vector imu_times(imu_traj.cols()); - std::vector 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(); 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& imu_traj = frame->imu_rate_trajectory; + + std::vector imu_times(imu_traj.cols()); + std::vector 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().head<3>(); + } + + if (!raw_points->times.empty()) { + std::vector 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>("time", times_f.data(), times_f.size()); + ply.properties.push_back(time_prop); + + std::vector 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>("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 diff --git a/modules/odometry/deskewing/src/glim_ext/deskewing_module_ros2.cpp b/modules/odometry/deskewing/src/glim_ext/deskewing_module_ros2.cpp index cca9077..9396767 100644 --- a/modules/odometry/deskewing/src/glim_ext/deskewing_module_ros2.cpp +++ b/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 imu_begin = imu_rate_traj.col(0); const Eigen::Matrix imu_end = imu_rate_traj.col(imu_rate_traj.cols() - 1); diff --git a/modules/odometry/fastlio2/include/glim_ext/odometry_estimation_fastlio2.hpp b/modules/odometry/fastlio2/include/glim_ext/odometry_estimation_fastlio2.hpp index d183107..8da6692 100644 --- a/modules/odometry/fastlio2/include/glim_ext/odometry_estimation_fastlio2.hpp +++ b/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; }; diff --git a/modules/odometry/fastlio2/src/glim_ext/odometry_estimation_fastlio2.cpp b/modules/odometry/fastlio2/src/glim_ext/odometry_estimation_fastlio2.cpp index 997fb46..8e82dbe 100644 --- a/modules/odometry/fastlio2/src/glim_ext/odometry_estimation_fastlio2.cpp +++ b/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("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity()); + const double imu_acc_std = sensor_config.param("sensors", "imu_acc_noise", 0.05); + const double imu_gyr_std = sensor_config.param("sensors", "imu_gyro_noise", 0.01); + const double imu_int_std = sensor_config.param("sensors", "imu_int_noise", 0.001); + const double imu_bias_acc_std = sensor_config.param("sensors", "imu_bias_noise_acc", 1e-5); + const double imu_bias_gyr_std = sensor_config.param("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("odometry_estimation", "cube_side_length", 1000.0); det_range = config.param("odometry_estimation", "det_range", 300.0); - gyr_cov = config.param("odometry_estimation", "gyr_cov", 0.1); - acc_cov = config.param("odometry_estimation", "acc_cov", 0.1); - b_gyr_cov = config.param("odometry_estimation", "b_gyr_cov", 0.0001); - b_acc_cov = config.param("odometry_estimation", "b_acc_cov", 0.0001); - extrinsic_est_en = config.param("odometry_estimation", "extrinsic_est_en", false); }