regulate the number of submap points

config/config_sub_mapping_gpu.json

@@ -23,6 +23,7 @@
   // --- Post processing ---
   // submap_downsample_resolution   : Resolution of voxel grid downsampling for created submaps
   // submap_voxel_resolution        : [deprecated] Resolution of VGICP voxels for created submaps (used in global mapping)
+  // submap_target_num_points       : Downsampling target number of points for created submaps (disabled when < 0)
   */
   "sub_mapping": {
     "so_name": "libsub_mapping.so",
@@ -46,6 +47,7 @@
     "keyframe_voxelmap_scaling_factor": 2.0,
     // Post processing
     "submap_downsample_resolution": 0.1,
-    "submap_voxel_resolution": 0.5
+    "submap_voxel_resolution": 0.5,
+    "submap_target_num_points": 50000
   }
 }

config/config_sub_mapping_passthrough.json

@@ -14,6 +14,7 @@
   // submap_voxel_resolution      : Resolution of submap voxels for downsampling
   // min_dist_in_voxel            : Minimum distance between points in a voxel
   // max_num_points_in_voxel      : Maximum number of points in a voxel
+  // submap_target_num_points     : Final target number of points for a submap (disabled when < 0)
   */
   "sub_mapping": {
     "so_name": "libsub_mapping_passthrough.so",
@@ -27,6 +28,7 @@
     // Submap creation params
     "submap_voxel_resolution": 0.5,
     "min_dist_in_voxel": 0.2,
-    "max_num_points_in_voxel": 100
+    "max_num_points_in_voxel": 100,
+    "submap_target_num_points": 50000
   }
 }

include/glim/mapping/sub_mapping.hpp

@@ -49,6 +49,7 @@ struct SubMappingParams {
 
   double submap_downsample_resolution;
   double submap_voxel_resolution;
+  int submap_target_num_points;
 };
 
 /**

include/glim/mapping/sub_mapping_passthrough.hpp

@@ -29,6 +29,7 @@ struct SubMappingPassthroughParams {
   int max_num_voxels;
   double adaptive_max_num_voxels;
 
+  int submap_target_num_points;
   double submap_voxel_resolution;
   double min_dist_in_voxel;
   int max_num_points_in_voxel;

src/glim/mapping/global_mapping.cpp

@@ -240,7 +240,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);
       auto voxelmap = std::make_shared<gtsam_points::GaussianVoxelMapGPU>(resolution);
-      voxelmap->insert(*subsampled_submap);
+      voxelmap->insert(*submap->frame);
       submap->voxelmaps.push_back(voxelmap);
     }
   }

src/glim/mapping/sub_mapping.cpp

@@ -58,6 +58,7 @@ SubMappingParams::SubMappingParams() {
 
   submap_downsample_resolution = config.param<double>("sub_mapping", "submap_downsample_resolution", 0.25);
   submap_voxel_resolution = config.param<double>("sub_mapping", "submap_voxel_resolution", 0.5);
+  submap_target_num_points = config.param<int>("sub_mapping", "submap_target_num_points", -1);
 
   enable_gpu = false;
   if (registration_error_factor_type.find("GPU") != std::string::npos) {
@@ -463,6 +464,12 @@ SubMap::Ptr SubMapping::create_submap(bool force_create) const {
   }
   logger->debug("|merged_submap|={}", submap->frame->size());
 
+  if (params.submap_target_num_points > 0 && submap->frame->size() > params.submap_target_num_points) {
+    std::mt19937 mt(submap_count * 643145 + submap->frame->size() * 4312);  // Just a random-like seed
+    submap->frame = gtsam_points::random_sampling(submap->frame, static_cast<double>(params.submap_target_num_points) / submap->frame->size(), mt);
+    logger->debug("|subsampled_submap|={}", submap->frame->size());
+  }
+
   return submap;
 }
 

src/glim/mapping/sub_mapping_passthrough.cpp

@@ -28,6 +28,7 @@ SubMappingPassthroughParams::SubMappingPassthroughParams() {
   adaptive_max_num_voxels = config.param<double>("sub_mapping", "adaptive_max_num_voxels", 0.5);
   adaptive_max_num_voxels = adaptive_max_num_voxels < 0 ? std::numeric_limits<double>::max() : adaptive_max_num_voxels;
 
+  submap_target_num_points = config.param<int>("sub_mapping", "submap_target_num_points", 40000);
   submap_voxel_resolution = config.param<double>("sub_mapping", "submap_voxel_resolution", 0.5);
   min_dist_in_voxel = config.param<double>("sub_mapping", "min_dist_in_voxel", 0.1);
   max_num_points_in_voxel = config.param<int>("sub_mapping", "max_num_points_in_voxel", 100);
@@ -145,6 +146,12 @@ SubMap::Ptr SubMappingPassthrough::create_submap(bool force_create) const {
   auto merged = voxelmap->voxel_data();
   submap->frame = gtsam_points::transform(merged, submap->T_world_origin.inverse());
 
+  if (params.submap_target_num_points > 0 && submap->frame->size() > params.submap_target_num_points) {
+    std::mt19937 mt(submap_count * 643145 + submap->frame->size() * 4312);  // Just a random-like seed
+    submap->frame = gtsam_points::random_sampling(submap->frame, static_cast<double>(params.submap_target_num_points) / submap->frame->size(), mt);
+    logger->debug("|subsampled_submap|={}", submap->frame->size());
+  }
+
   return submap;
 }