From 97ac6e6759e2d6855851606980f9f0b98a3cfe2d Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Tue, 7 Jul 2026 15:27:45 +0900 Subject: [PATCH 1/3] fix backface culling for keyframes --- src/glim/viewer/standard_viewer_callbacks.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/glim/viewer/standard_viewer_callbacks.cpp b/src/glim/viewer/standard_viewer_callbacks.cpp index 1a200145..eacc4d9e 100644 --- a/src/glim/viewer/standard_viewer_callbacks.cpp +++ b/src/glim/viewer/standard_viewer_callbacks.cpp @@ -189,6 +189,9 @@ void StandardViewer::set_callbacks() { auto drawable = viewer->find_drawable(name); if (drawable.first == nullptr) { auto cloud_buffer = std::make_shared(keyframe->frame->points, keyframe->frame->size()); + if (keyframe->frame->has_normals()) { + cloud_buffer->add_normals(keyframe->frame->normals, keyframe->frame->size()); + } if (keyframe->frame->has_intensities()) { std::vector intensities(keyframe->frame->intensities, keyframe->frame->intensities + keyframe->frame->size()); cloud_buffer->add_colormap(intensities); From 5c22a19b763001391967f2dc492b3a55447c6817 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Tue, 7 Jul 2026 17:43:27 +0900 Subject: [PATCH 2/3] compute covs --- config/config_odometry_cpu.json | 1 + config/config_odometry_ct.json | 2 ++ config/config_odometry_gpu.json | 2 ++ include/glim/odometry/callbacks.hpp | 8 ++++++++ include/glim/odometry/estimation_frame.hpp | 15 +++++++++++---- include/glim/odometry/odometry_estimation_ct.hpp | 1 + include/glim/odometry/odometry_estimation_imu.hpp | 3 +++ src/glim/odometry/callbacks.cpp | 2 ++ src/glim/odometry/estimation_frame.cpp | 12 ++++++++++++ src/glim/odometry/odometry_estimation_ct.cpp | 11 +++++++++++ src/glim/odometry/odometry_estimation_imu.cpp | 15 +++++++++++++++ 11 files changed, 68 insertions(+), 4 deletions(-) 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..b13fb648 100644 --- a/src/glim/odometry/odometry_estimation_ct.cpp +++ b/src/glim/odometry/odometry_estimation_ct.cpp @@ -72,6 +72,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 +273,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 211160fb..58fc6e34 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() {} @@ -410,6 +417,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( From 0006572227b25434ec8f04d8e54dd003fa97532f Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Tue, 7 Jul 2026 17:55:17 +0900 Subject: [PATCH 3/3] fix uninitialized compute_covs --- src/glim/odometry/odometry_estimation_ct.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/glim/odometry/odometry_estimation_ct.cpp b/src/glim/odometry/odometry_estimation_ct.cpp index b13fb648..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() {}