diff --git a/config/config_odometry_cpu.json b/config/config_odometry_cpu.json index c5017148..be7585c6 100644 --- a/config/config_odometry_cpu.json +++ b/config/config_odometry_cpu.json @@ -17,6 +17,7 @@ "isam2_relinearize_skip": 1, "isam2_relinearize_thresh": 0.1, "fix_imu_bias": false, + "compute_covs": false, // Registration settings "registration_type": "GICP", "max_iterations": 8, diff --git a/config/config_odometry_ct.json b/config/config_odometry_ct.json index 4b01704b..2d15a3da 100644 --- a/config/config_odometry_ct.json +++ b/config/config_odometry_ct.json @@ -4,6 +4,7 @@ // ivox_resolution : Target iVox resolution // ivox_min_points_dist : Minimum distance between points in an iVox cell // ivox_lru_thresh : LRC cache threshold + // compute_covs : If true, compute marginal covariances of the latest frame (takes a few milliseconds per frame) // max_correspondence_distance : Maximum distance for ICP correspondence search // location_consistency_inf_scale : Weight for location consistency constraints // constant_velocity_inf_scale : Weight for constant velocity constraints @@ -29,6 +30,7 @@ "use_isam2_dogleg": false, "isam2_relinearize_skip": 1, "isam2_relinearize_thresh": 0.1, + "compute_covs": false, // Misc "num_threads": 4 } diff --git a/config/config_odometry_gpu.json b/config/config_odometry_gpu.json index a968c89f..c1b88c12 100644 --- a/config/config_odometry_gpu.json +++ b/config/config_odometry_gpu.json @@ -12,6 +12,7 @@ // isam2_relinearize_skip : // isam2_relinearize_thresh : // fix_imu_bias : If true, disable IMU bias estimation and use the initial IMU bias + // compute_covs : If true, compute marginal covariances of the latest frame (takes a few milliseconds per frame) // // --- Voxel params --- // voxel_resolution : Base resolution for VGICP voxels @@ -48,6 +49,7 @@ "isam2_relinearize_skip": 1, "isam2_relinearize_thresh": 0.1, "fix_imu_bias": false, + "compute_covs": false, // Voxel params "voxel_resolution": 0.25, "voxel_resolution_max": 0.5, diff --git a/include/glim/odometry/callbacks.hpp b/include/glim/odometry/callbacks.hpp index f1e8cada..06e0b52e 100644 --- a/include/glim/odometry/callbacks.hpp +++ b/include/glim/odometry/callbacks.hpp @@ -134,6 +134,14 @@ struct OdometryEstimationCallbacks { * @note We should not forget that FixedLagSmoothers are in "gtsam_unstable" directory!! */ static CallbackSlot on_smoother_corruption; + + /** + * @brief Request to compute marginal covariances of the latest frame (X(i), V(i), B(i)). + * @note This persists throughout the lifecycle of the odometry estimation instance. + * @note This should only be called in the initialization of extension modules. + * Calling this while the odometry estimation is running may lead to undefined behavior. + */ + static CallbackSlot request_to_compute_covariances; }; } // namespace glim \ No newline at end of file diff --git a/include/glim/odometry/estimation_frame.hpp b/include/glim/odometry/estimation_frame.hpp index add5795a..84274026 100644 --- a/include/glim/odometry/estimation_frame.hpp +++ b/include/glim/odometry/estimation_frame.hpp @@ -23,6 +23,9 @@ struct EstimationFrame { EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EstimationFrame(); + ~EstimationFrame(); + /** * @brief Make a clone of the estimation frame. (Points data are shallow copied) * @return EstimationFrame::Ptr Cloned frame @@ -82,13 +85,17 @@ struct EstimationFrame { long id; ///< Frame ID double stamp; ///< Timestamp - Eigen::Isometry3d T_lidar_imu; ///< LiDAR-IMU transformation - Eigen::Isometry3d T_world_lidar; ///< LiDAR pose in the world space - Eigen::Isometry3d T_world_imu; ///< IMU pose in the world space - + Eigen::Isometry3d T_lidar_imu; ///< LiDAR-IMU transformation + Eigen::Isometry3d T_world_lidar; ///< LiDAR pose in the world space + Eigen::Isometry3d T_world_imu; ///< IMU pose in the world space Eigen::Vector3d v_world_imu; ///< IMU velocity in the world frame Eigen::Matrix imu_bias; ///< IMU bias + bool cov_computed; ///< Whether the covariances have been computed + Eigen::Matrix pose_cov; ///< Pose covariance (X(i) in the graph. The reference frame depends on frame_id) + Eigen::Matrix vel_cov; ///< Velocity covariance (V(i) in the graph) + Eigen::Matrix bias_cov; ///< IMU bias covariance (B(i) in the graph) + PreprocessedFrame::ConstPtr raw_frame; ///< Raw input point cloud (LiDAR frame) Eigen::Matrix imu_rate_trajectory; ///< IMU-rate trajectory 8 x N [t, x, y, z, qx, qy, qz, qw] diff --git a/include/glim/odometry/odometry_estimation_ct.hpp b/include/glim/odometry/odometry_estimation_ct.hpp index 5e835182..cfdf945d 100644 --- a/include/glim/odometry/odometry_estimation_ct.hpp +++ b/include/glim/odometry/odometry_estimation_ct.hpp @@ -51,6 +51,7 @@ struct OdometryEstimationCTParams { bool use_isam2_dogleg; ///< If true, use dogleg optimizer double isam2_relinearize_skip; double isam2_relinearize_thresh; + bool compute_covs; ///< If true, compute marginal covariances of the latest frame. }; /** diff --git a/include/glim/odometry/odometry_estimation_imu.hpp b/include/glim/odometry/odometry_estimation_imu.hpp index 340a74a5..ac7cca7c 100644 --- a/include/glim/odometry/odometry_estimation_imu.hpp +++ b/include/glim/odometry/odometry_estimation_imu.hpp @@ -58,6 +58,9 @@ struct OdometryEstimationIMUParams { double isam2_relinearize_skip; double isam2_relinearize_thresh; + // Marginal computation params + bool compute_covs; // Whether to compute marginal covariances of the latest frame (X(i), V(i), B(i)) + // Logging params bool validate_imu; bool save_imu_rate_trajectory; diff --git a/src/glim/odometry/callbacks.cpp b/src/glim/odometry/callbacks.cpp index bd2843aa..9c9d824e 100644 --- a/src/glim/odometry/callbacks.cpp +++ b/src/glim/odometry/callbacks.cpp @@ -29,4 +29,6 @@ CallbackSlot OdometryEstimationCallbacks::on_smoother_corruption; +CallbackSlot OdometryEstimationCallbacks::request_to_compute_covariances; + } // namespace glim \ No newline at end of file diff --git a/src/glim/odometry/estimation_frame.cpp b/src/glim/odometry/estimation_frame.cpp index 03bdd2b1..e617e85e 100644 --- a/src/glim/odometry/estimation_frame.cpp +++ b/src/glim/odometry/estimation_frame.cpp @@ -4,6 +4,18 @@ namespace glim { +EstimationFrame::EstimationFrame() { + id = -1; ///< Frame ID + stamp = -1.0; ///< Timestamp + + cov_computed = false; + pose_cov.setZero(); + vel_cov.setZero(); + bias_cov.setZero(); +} + +EstimationFrame::~EstimationFrame() {} + EstimationFrame::Ptr EstimationFrame::clone() const { EstimationFrame::Ptr cloned(new EstimationFrame); *cloned = *this; diff --git a/src/glim/odometry/odometry_estimation_ct.cpp b/src/glim/odometry/odometry_estimation_ct.cpp index 8c34db77..b3ad9866 100644 --- a/src/glim/odometry/odometry_estimation_ct.cpp +++ b/src/glim/odometry/odometry_estimation_ct.cpp @@ -48,6 +48,7 @@ OdometryEstimationCTParams::OdometryEstimationCTParams() { use_isam2_dogleg = config.param("odometry_estimation", "use_isam2_dogleg", false); isam2_relinearize_skip = config.param("odometry_estimation", "isam2_relinearize_skip", 1); isam2_relinearize_thresh = config.param("odometry_estimation", "isam2_relinearize_thresh", 0.1); + compute_covs = config.param("odometry_estimation", "compute_covs", false); } OdometryEstimationCTParams::~OdometryEstimationCTParams() {} @@ -72,6 +73,11 @@ OdometryEstimationCT::OdometryEstimationCT(const OdometryEstimationCTParams& par #ifdef GTSAM_USE_TBB tbb_task_arena = std::make_shared(1); #endif + + Callbacks::request_to_compute_covariances.add([this]() { + logger->debug("request to compute marginal covs"); + this->params.compute_covs = true; + }); } OdometryEstimationCT::~OdometryEstimationCT() {} @@ -268,6 +274,12 @@ EstimationFrame::ConstPtr OdometryEstimationCT::insert_frame(const PreprocessedF } } + if (params.compute_covs) { + logger->debug("computing marginal covariances of the latest frame"); + new_frame->cov_computed = true; + new_frame->pose_cov = smoother->marginalCovariance(X(current)); + } + std::vector active_frames(frames.begin() + marginalized_cursor, frames.end()); Callbacks::on_update_new_frame(active_frames.back()); Callbacks::on_update_frames(active_frames); diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp index fae7fdd0..cd71721a 100644 --- a/src/glim/odometry/odometry_estimation_imu.cpp +++ b/src/glim/odometry/odometry_estimation_imu.cpp @@ -62,6 +62,8 @@ OdometryEstimationIMUParams::OdometryEstimationIMUParams() { isam2_relinearize_skip = config.param("odometry_estimation", "isam2_relinearize_skip", 1); isam2_relinearize_thresh = config.param("odometry_estimation", "isam2_relinearize_thresh", 0.1); + compute_covs = config.param("odometry_estimation", "compute_covs", false); + validate_imu = config.param("odometry_estimation", "validate_imu", true); save_imu_rate_trajectory = config.param("odometry_estimation", "save_imu_rate_trajectory", false); @@ -106,6 +108,11 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptr(params->num_smoother_update_threads); #endif + + Callbacks::request_to_compute_covariances.add([this]() { + logger->debug("request to compute marginal covs"); + params->compute_covs = true; + }); } OdometryEstimationIMU::~OdometryEstimationIMU() {} @@ -411,6 +418,14 @@ void OdometryEstimationIMU::update_frames(int current, const gtsam::NonlinearFac break; } } + + if (params->compute_covs) { + auto latest_frame = frames[current]; + latest_frame->cov_computed = true; + latest_frame->pose_cov = smoother->marginalCovariance(X(current)); + latest_frame->vel_cov = smoother->marginalCovariance(V(current)); + latest_frame->bias_cov = smoother->marginalCovariance(B(current)); + } } void OdometryEstimationIMU::update_smoother(