Compute covs

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,

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
   }

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,

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<void(double)> 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<void()> request_to_compute_covariances;
 };
 
 }  // namespace glim

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<double, 6, 1> imu_bias;  ///< IMU bias
 
+  bool cov_computed;                     ///< Whether the covariances have been computed
+  Eigen::Matrix<double, 6, 6> pose_cov;  ///< Pose covariance (X(i) in the graph. The reference frame depends on frame_id)
+  Eigen::Matrix<double, 3, 3> vel_cov;   ///< Velocity covariance (V(i) in the graph)
+  Eigen::Matrix<double, 6, 6> bias_cov;  ///< IMU bias covariance (B(i) in the graph)
+
   PreprocessedFrame::ConstPtr raw_frame;             ///< Raw input point cloud (LiDAR frame)
   Eigen::Matrix<double, 8, -1> imu_rate_trajectory;  ///< IMU-rate trajectory 8 x N  [t, x, y, z, qx, qy, qz, qw]
 

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.
 };
 
 /**

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;

src/glim/odometry/callbacks.cpp

@@ -29,4 +29,6 @@ CallbackSlot<void(gtsam_points::IncrementalFixedLagSmootherExtWithFallback& smoo
 
 CallbackSlot<void(double)> OdometryEstimationCallbacks::on_smoother_corruption;
 
+CallbackSlot<void()> OdometryEstimationCallbacks::request_to_compute_covariances;
+
 }  // namespace glim

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;

src/glim/odometry/odometry_estimation_ct.cpp

@@ -48,6 +48,7 @@ OdometryEstimationCTParams::OdometryEstimationCTParams() {
   use_isam2_dogleg = config.param<bool>("odometry_estimation", "use_isam2_dogleg", false);
   isam2_relinearize_skip = config.param<int>("odometry_estimation", "isam2_relinearize_skip", 1);
   isam2_relinearize_thresh = config.param<double>("odometry_estimation", "isam2_relinearize_thresh", 0.1);
+  compute_covs = config.param<bool>("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<tbb::task_arena>(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<EstimationFrame::ConstPtr> active_frames(frames.begin() + marginalized_cursor, frames.end());
   Callbacks::on_update_new_frame(active_frames.back());
   Callbacks::on_update_frames(active_frames);

src/glim/odometry/odometry_estimation_imu.cpp

@@ -62,6 +62,8 @@ OdometryEstimationIMUParams::OdometryEstimationIMUParams() {
   isam2_relinearize_skip = config.param<int>("odometry_estimation", "isam2_relinearize_skip", 1);
   isam2_relinearize_thresh = config.param<double>("odometry_estimation", "isam2_relinearize_thresh", 0.1);
 
+  compute_covs = config.param<bool>("odometry_estimation", "compute_covs", false);
+
   validate_imu = config.param<bool>("odometry_estimation", "validate_imu", true);
   save_imu_rate_trajectory = config.param<bool>("odometry_estimation", "save_imu_rate_trajectory", false);
 
@@ -106,6 +108,11 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptr<OdometryEstimationI
 #ifdef GTSAM_USE_TBB
   tbb_task_arena = std::make_shared<tbb::task_arena>(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(