add plane_eps

koide3 · Aug 29, 2024 · da99e10 · da99e10
1 parent 2dcf50b
commit da99e10
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 6 deletions.
diff --git a/config/config_preprocess.json b/config/config_preprocess.json
@@ -10,6 +10,7 @@
   // outlier_removal_k           : Number of neighbors for outlier removal
   // outlier_std_mul_factor      : Standard deviation multiplier for outlier removal
   // k_correspondences           : Number of neighbors for covariance estimation
+  // plane_eps                   : Epsilon for covariance regularization (plane thickness)
   // num_threads                 : Number of threads for preprocessing
   */
   "preprocess": {
@@ -23,6 +24,7 @@
     "outlier_removal_k": 10,
     "outlier_std_mul_factor": 1.0,
     "k_correspondences": 10,
+    "plane_eps": 1e-3,
     "num_threads": 2
   }
 }
diff --git a/include/glim/common/cloud_covariance_estimation.hpp b/include/glim/common/cloud_covariance_estimation.hpp
@@ -15,7 +15,7 @@ enum class RegularizationMethod { NONE, PLANE, NORMALIZED_MIN_EIG, FROBENIUS };
  */
 class CloudCovarianceEstimation {
 public:
-  CloudCovarianceEstimation(const int num_threads = 1);
+  CloudCovarianceEstimation(const int num_threads = 1, const double plane_eps = 1e-3);
   ~CloudCovarianceEstimation();
 
   /**
@@ -60,6 +60,7 @@ class CloudCovarianceEstimation {
 private:
   const RegularizationMethod regularization_method;
   const int num_threads;
+  const double plane_eps;
 };
 
 }  // namespace glim
diff --git a/include/glim/odometry/odometry_estimation_imu.hpp b/include/glim/odometry/odometry_estimation_imu.hpp
@@ -37,6 +37,9 @@ struct OdometryEstimationIMUParams {
   virtual ~OdometryEstimationIMUParams();
 
 public:
+  // Preprocessing params
+  double plane_eps;
+
   // Sensor params;
   bool fix_imu_bias;
   double imu_bias_noise;

diff --git a/src/glim/common/cloud_covariance_estimation.cpp b/src/glim/common/cloud_covariance_estimation.cpp
@@ -10,7 +10,10 @@
 
 namespace glim {
 
-CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads) : regularization_method(RegularizationMethod::PLANE), num_threads(num_threads) {}
+CloudCovarianceEstimation::CloudCovarianceEstimation(const int num_threads, const double plane_eps)
+: regularization_method(RegularizationMethod::PLANE),
+  num_threads(num_threads),
+  plane_eps(plane_eps) {}
 
 CloudCovarianceEstimation::~CloudCovarianceEstimation() {}
 
@@ -151,7 +154,7 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov
         *eigenvectors = eig.eigenvectors();
       }
 
-      Eigen::Vector3d values(1e-3, 1.0, 1.0);
+      Eigen::Vector3d values(plane_eps, 1.0, 1.0);
       Eigen::Matrix4d c = Eigen::Matrix4d::Zero();
       c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose();
       return c;
@@ -169,15 +172,15 @@ Eigen::Matrix4d CloudCovarianceEstimation::regularize(const Eigen::Matrix4d& cov
       }
 
       Eigen::Vector3d values = eig.eigenvalues() / eig.eigenvalues()[2];
-      values = values.array().max(1e-3);
+      values = values.array().max(plane_eps);
 
       Eigen::Matrix4d c = Eigen::Matrix4d::Zero();
       c.block<3, 3>(0, 0) = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().transpose();
       return c;
     }
 
     case RegularizationMethod::FROBENIUS: {
-      const double lambda = 1e-3;
+      const double lambda = plane_eps;
       Eigen::Matrix3d C = cov.block<3, 3>(0, 0) + lambda * Eigen::Matrix3d::Identity();
       Eigen::Matrix3d C_inv = C.inverse();
       Eigen::Matrix4d C_ = Eigen::Matrix4d::Zero();

diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp
@@ -32,6 +32,9 @@ using gtsam::symbol_shorthand::V;  // IMU velocity   (v_world_imu)
 using gtsam::symbol_shorthand::X;  // IMU pose       (T_world_imu)
 
 OdometryEstimationIMUParams::OdometryEstimationIMUParams() {
+  Config preprocess_config(GlobalConfig::get_config_path("config_preprocess"));
+  plane_eps = preprocess_config.param<double>("preprocess", "plane_eps", 1e-3);
+
   // sensor config
   Config sensor_config(GlobalConfig::get_config_path("config_sensors"));
   T_lidar_imu = sensor_config.param<Eigen::Isometry3d>("sensors", "T_lidar_imu", Eigen::Isometry3d::Identity());
@@ -89,7 +92,7 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptr<OdometryEstimationI
 
   imu_integration.reset(new IMUIntegration);
   deskewing.reset(new CloudDeskewing);
-  covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads));
+  covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads, params->plane_eps));
 
   gtsam::ISAM2Params isam2_params;
   if (params->use_isam2_dogleg) {