adaptive voxel resolution (#97)

* adaptive voxel resolution

* adaptive voxel resolution for global mapping

* use median_distance in gtsam_points

config/config_global_mapping_gpu.json

@@ -36,6 +36,9 @@
     "registration_error_factor_type": "VGICP_GPU",
     "randomsampling_rate": 1.0,
     "submap_voxel_resolution": 0.5,
+    "submap_voxel_resolution_max": 2.0,
+    "submap_voxel_resolution_dmin": 5.0,
+    "submap_voxel_resolution_dmax": 20.0,
     "submap_voxelmap_levels": 2,
     "submap_voxelmap_scaling_factor": 2.0,
     "max_implicit_loop_distance": 100.0,

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": 5.0,
+    "voxel_resolution_dmax": 20.0,
     "voxelmap_levels": 2,
     "voxelmap_scaling_factor": 2.0,
     "full_connection_window_size": 2,

include/glim/mapping/global_mapping.hpp

@@ -38,6 +38,9 @@ struct GlobalMappingParams {
 
   std::string registration_error_factor_type;
   double submap_voxel_resolution;
+  double submap_voxel_resolution_max;
+  double submap_voxel_resolution_dmin;
+  double submap_voxel_resolution_dmax;
   int submap_voxelmap_levels;
   double submap_voxelmap_scaling_factor;
 

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/mapping/global_mapping.cpp

@@ -54,6 +54,10 @@ GlobalMappingParams::GlobalMappingParams() {
   between_registration_type = config.param<std::string>("global_mapping", "between_registration_type", "GICP");
   registration_error_factor_type = config.param<std::string>("global_mapping", "registration_error_factor_type", "VGICP");
   submap_voxel_resolution = config.param<double>("global_mapping", "submap_voxel_resolution", 1.0);
+  submap_voxel_resolution_max = config.param<double>("global_mapping", "submap_voxel_resolution_max", submap_voxel_resolution);
+  submap_voxel_resolution_dmin = config.param<double>("global_mapping", "submap_voxel_resolution_dmin", 5.0);
+  submap_voxel_resolution_dmax = config.param<double>("global_mapping", "submap_voxel_resolution_dmax", 20.0);
+
   submap_voxelmap_levels = config.param<int>("global_mapping", "submap_voxelmap_levels", 2);
   submap_voxelmap_scaling_factor = config.param<double>("global_mapping", "submap_voxelmap_scaling_factor", 2.0);
 
@@ -224,6 +228,13 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) {
 void GlobalMapping::insert_submap(int current, const SubMap::Ptr& submap) {
   submap->voxelmaps.clear();
 
+  // Adaptively determine the voxel resolution based on the median distance
+  const int max_scan_count = 256;
+  const double dist_median = gtsam_points::median_distance(submap->frame, max_scan_count);
+  const double p = std::max(0.0, std::min(1.0, (dist_median - params.submap_voxel_resolution_dmin) / (params.submap_voxel_resolution_dmax - params.submap_voxel_resolution_dmin)));
+  const double base_resolution = params.submap_voxel_resolution + p * (params.submap_voxel_resolution_max - params.submap_voxel_resolution);
+
+  // Create frame and voxelmaps
   gtsam_points::PointCloud::ConstPtr subsampled_submap;
   if (params.randomsampling_rate > 0.99) {
     subsampled_submap = submap->frame;
@@ -244,7 +255,7 @@ void GlobalMapping::insert_submap(int current, const SubMap::Ptr& submap) {
     }
 
     for (int i = 0; i < params.submap_voxelmap_levels; i++) {
-      const double resolution = params.submap_voxel_resolution * std::pow(params.submap_voxelmap_scaling_factor, i);
+      const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, i);
       auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapGPU>(resolution);
       voxelmap->insert(*submap->frame);
       submap->voxelmaps.push_back(voxelmap);
@@ -254,7 +265,7 @@ void GlobalMapping::insert_submap(int current, const SubMap::Ptr& submap) {
 
   if (submap->voxelmaps.empty()) {
     for (int i = 0; i < params.submap_voxelmap_levels; i++) {
-      const double resolution = params.submap_voxel_resolution * std::pow(params.submap_voxelmap_scaling_factor, i);
+      const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, i);
       auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapCPU>(resolution);
       voxelmap->insert(*subsampled_submap);
       submap->voxelmaps.push_back(voxelmap);
@@ -652,7 +663,19 @@ bool GlobalMapping::load(const std::string& path) {
       return false;
     }
 
-    gtsam_points::PointCloud::Ptr subsampled_submap = gtsam_points::random_sampling(submap->frame, params.randomsampling_rate, mt);
+    // Adaptively determine the voxel resolution based on the median distance
+    const int max_scan_count = 256;
+    const double dist_median = gtsam_points::median_distance(submap->frame, max_scan_count);
+    const double p =
+      std::max(0.0, std::min(1.0, (dist_median - params.submap_voxel_resolution_dmin) / (params.submap_voxel_resolution_dmax - params.submap_voxel_resolution_dmin)));
+    const double base_resolution = params.submap_voxel_resolution + p * (params.submap_voxel_resolution_max - params.submap_voxel_resolution);
+
+    gtsam_points::PointCloud::Ptr subsampled_submap;
+    if (params.randomsampling_rate > 0.99) {
+      subsampled_submap = submap->frame;
+    } else {
+      subsampled_submap = gtsam_points::random_sampling(submap->frame, params.randomsampling_rate, mt);
+    }
 
     submaps[i] = submap;
     submaps[i]->voxelmaps.clear();
@@ -663,7 +686,7 @@ bool GlobalMapping::load(const std::string& path) {
       subsampled_submaps[i] = gtsam_points::PointCloudGPU::clone(*subsampled_submaps[i]);
 
       for (int j = 0; j < params.submap_voxelmap_levels; j++) {
-        const double resolution = params.submap_voxel_resolution * std::pow(params.submap_voxelmap_scaling_factor, j);
+        const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, j);
         auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapGPU>(resolution);
         voxelmap->insert(*subsampled_submaps[i]);
         submaps[i]->voxelmaps.push_back(voxelmap);
@@ -673,7 +696,7 @@ bool GlobalMapping::load(const std::string& path) {
 #endif
     } else {
       for (int j = 0; j < params.submap_voxelmap_levels; j++) {
-        const double resolution = params.submap_voxel_resolution * std::pow(params.submap_voxelmap_scaling_factor, j);
+        const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, j);
         auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapCPU>(resolution);
         voxelmap->insert(*subsampled_submaps[i]);
         submaps[i]->voxelmaps.push_back(voxelmap);

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,20 @@ 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 = 256;
+  const double dist_median = gtsam_points::median_distance(new_frame->frame, max_scan_count);
+  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);