diff --git a/config/config_sub_mapping_gpu.json b/config/config_sub_mapping_gpu.json index ef200ba0..242dace3 100644 --- a/config/config_sub_mapping_gpu.json +++ b/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 } } \ No newline at end of file diff --git a/config/config_sub_mapping_passthrough.json b/config/config_sub_mapping_passthrough.json index 2c5f00a6..10f70ec1 100644 --- a/config/config_sub_mapping_passthrough.json +++ b/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 } } \ No newline at end of file diff --git a/include/glim/mapping/sub_mapping.hpp b/include/glim/mapping/sub_mapping.hpp index bace2edd..6c1b1a30 100644 --- a/include/glim/mapping/sub_mapping.hpp +++ b/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; }; /** diff --git a/include/glim/mapping/sub_mapping_passthrough.hpp b/include/glim/mapping/sub_mapping_passthrough.hpp index a89d3db3..2c1b24ab 100644 --- a/include/glim/mapping/sub_mapping_passthrough.hpp +++ b/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; diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index c7648528..202e1dbb 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/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(resolution); - voxelmap->insert(*subsampled_submap); + voxelmap->insert(*submap->frame); submap->voxelmaps.push_back(voxelmap); } } diff --git a/src/glim/mapping/sub_mapping.cpp b/src/glim/mapping/sub_mapping.cpp index 491e0b5f..2787fa6d 100644 --- a/src/glim/mapping/sub_mapping.cpp +++ b/src/glim/mapping/sub_mapping.cpp @@ -58,6 +58,7 @@ SubMappingParams::SubMappingParams() { submap_downsample_resolution = config.param("sub_mapping", "submap_downsample_resolution", 0.25); submap_voxel_resolution = config.param("sub_mapping", "submap_voxel_resolution", 0.5); + submap_target_num_points = config.param("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(params.submap_target_num_points) / submap->frame->size(), mt); + logger->debug("|subsampled_submap|={}", submap->frame->size()); + } + return submap; } diff --git a/src/glim/mapping/sub_mapping_passthrough.cpp b/src/glim/mapping/sub_mapping_passthrough.cpp index 6fc1f1b7..b8a35149 100644 --- a/src/glim/mapping/sub_mapping_passthrough.cpp +++ b/src/glim/mapping/sub_mapping_passthrough.cpp @@ -28,6 +28,7 @@ SubMappingPassthroughParams::SubMappingPassthroughParams() { adaptive_max_num_voxels = config.param("sub_mapping", "adaptive_max_num_voxels", 0.5); adaptive_max_num_voxels = adaptive_max_num_voxels < 0 ? std::numeric_limits::max() : adaptive_max_num_voxels; + submap_target_num_points = config.param("sub_mapping", "submap_target_num_points", 40000); submap_voxel_resolution = config.param("sub_mapping", "submap_voxel_resolution", 0.5); min_dist_in_voxel = config.param("sub_mapping", "min_dist_in_voxel", 0.1); max_num_points_in_voxel = config.param("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(params.submap_target_num_points) / submap->frame->size(), mt); + logger->debug("|subsampled_submap|={}", submap->frame->size()); + } + return submap; }