adaptive voxel resolution

config/config_odometry_gpu.json

@@ -14,7 +14,10 @@
   // fix_imu_bias                : If true, disable IMU bias estimation and use the initial IMU bias
   // 
   // --- Voxel params ---
-  // voxel_resolution            : Resolution for VGICP voxels (0.25 ~ 0.5 for indoor, 0.5 ~ 2.0 for outdoor)
+  // voxel_resolution            : Base resolution for VGICP voxels
+  // voxel_resolution_max        : Maximum base resolution for VGICP voxels.
+  //                             : When voxel_resolution_max > voxel_resolution, the voxel resolution is adaptively increased
+  //                             : depending on the median of point distances clamped by dmin and dmax.
   // voxelmap_levels             : Multi-resolution voxelmap levels
   // voxelmap_scaling_factor     : Multi-resolution voxelmap scaling factor
   // full_connection_window_size : Latest sensor pose variable is connected with all of the past $full_connection_window_size poses
@@ -46,6 +49,9 @@
     "fix_imu_bias": false,
     // Voxel params
     "voxel_resolution": 0.25,
+    "voxel_resolution_max": 1.0,
+    "voxel_resolution_dmin": 3.0,
+    "voxel_resolution_dmax": 20.0,
     "voxelmap_levels": 2,
     "voxelmap_scaling_factor": 2.0,
     "full_connection_window_size": 2,

include/glim/odometry/odometry_estimation_gpu.hpp

@@ -25,6 +25,9 @@ struct OdometryEstimationGPUParams : public OdometryEstimationIMUParams {
 public:
   // Registration params
   double voxel_resolution;
+  double voxel_resolution_max;
+  double voxel_resolution_dmin;
+  double voxel_resolution_dmax;
   int voxelmap_levels;
   double voxelmap_scaling_factor;
 

src/glim/odometry/odometry_estimation_gpu.cpp

@@ -39,6 +39,10 @@ OdometryEstimationGPUParams::OdometryEstimationGPUParams() : OdometryEstimationI
   Config config(GlobalConfig::get_config_path("config_odometry"));
 
   voxel_resolution = config.param<double>("odometry_estimation", "voxel_resolution", 0.5);
+  voxel_resolution_max = config.param<double>("odometry_estimation", "voxel_resolution_max", voxel_resolution);
+  voxel_resolution_dmin = config.param<double>("odometry_estimation", "voxel_resolution_dmin", 4.0);
+  voxel_resolution_dmax = config.param<double>("odometry_estimation", "voxel_resolution_dmax", 12.0);
+
   voxelmap_levels = config.param<int>("odometry_estimation", "voxelmap_levels", 2);
   voxelmap_scaling_factor = config.param<double>("odometry_estimation", "voxelmap_scaling_factor", 2.0);
 
@@ -83,13 +87,27 @@ OdometryEstimationGPU::~OdometryEstimationGPU() {
 void OdometryEstimationGPU::create_frame(EstimationFrame::Ptr& new_frame) {
   const auto params = static_cast<OdometryEstimationGPUParams*>(this->params.get());
 
+  // Adaptively determine the voxel resolution based on the median distance
+  const int max_scan_count = 512;
+  const int step = new_frame->frame->size() < max_scan_count ? 1 : new_frame->frame->size() / max_scan_count;
+  std::vector<double> dists;
+  dists.reserve(max_scan_count * 2);
+  for (int i = 0; i < new_frame->frame->size(); i += step) {
+    dists.emplace_back(new_frame->frame->points[i].head<3>().norm());
+  }
+  std::nth_element(dists.begin(), dists.begin() + dists.size() / 2, dists.end());
+  const double dist_median = dists[dists.size() / 2];
+  const double p = std::max(0.0, std::min(1.0, (dist_median - params->voxel_resolution_dmin) / (params->voxel_resolution_dmax - params->voxel_resolution_dmin)));
+  const double base_resolution = params->voxel_resolution + p * (params->voxel_resolution_max - params->voxel_resolution);
+
+  // Create frame and voxelmaps
   new_frame->frame = gtsam_points::PointCloudGPU::clone(*new_frame->frame);
   for (int i = 0; i < params->voxelmap_levels; i++) {
     if (!new_frame->frame->size()) {
       break;
     }
 
-    const double resolution = params->voxel_resolution * std::pow(params->voxelmap_scaling_factor, i);
+    const double resolution = base_resolution * std::pow(params->voxelmap_scaling_factor, i);
     auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapGPU>(resolution, 8192 * 2, 10, 1e-3, *stream);
     voxelmap->insert(*new_frame->frame);
     new_frame->voxelmaps.push_back(voxelmap);