From 58188aaf43b98667ccb39894f2235a7bf33b2d61 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 29 Oct 2024 16:52:44 +0900 Subject: [PATCH] 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 | 3 ++ config/config_odometry_gpu.json | 8 ++++- include/glim/mapping/global_mapping.hpp | 3 ++ .../glim/odometry/odometry_estimation_gpu.hpp | 3 ++ src/glim/mapping/global_mapping.cpp | 33 ++++++++++++++++--- src/glim/odometry/odometry_estimation_gpu.cpp | 13 +++++++- 6 files changed, 56 insertions(+), 7 deletions(-) diff --git a/config/config_global_mapping_gpu.json b/config/config_global_mapping_gpu.json index d6169ad..3b31120 100644 --- a/config/config_global_mapping_gpu.json +++ b/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, diff --git a/config/config_odometry_gpu.json b/config/config_odometry_gpu.json index ddcc328..113a91c 100644 --- a/config/config_odometry_gpu.json +++ b/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, diff --git a/include/glim/mapping/global_mapping.hpp b/include/glim/mapping/global_mapping.hpp index 6e4c8eb..b3ea803 100644 --- a/include/glim/mapping/global_mapping.hpp +++ b/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; diff --git a/include/glim/odometry/odometry_estimation_gpu.hpp b/include/glim/odometry/odometry_estimation_gpu.hpp index 2d63161..e596ec6 100644 --- a/include/glim/odometry/odometry_estimation_gpu.hpp +++ b/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; diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index 136e1b6..d2bdd79 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/src/glim/mapping/global_mapping.cpp @@ -54,6 +54,10 @@ GlobalMappingParams::GlobalMappingParams() { between_registration_type = config.param("global_mapping", "between_registration_type", "GICP"); registration_error_factor_type = config.param("global_mapping", "registration_error_factor_type", "VGICP"); submap_voxel_resolution = config.param("global_mapping", "submap_voxel_resolution", 1.0); + submap_voxel_resolution_max = config.param("global_mapping", "submap_voxel_resolution_max", submap_voxel_resolution); + submap_voxel_resolution_dmin = config.param("global_mapping", "submap_voxel_resolution_dmin", 5.0); + submap_voxel_resolution_dmax = config.param("global_mapping", "submap_voxel_resolution_dmax", 20.0); + submap_voxelmap_levels = config.param("global_mapping", "submap_voxelmap_levels", 2); submap_voxelmap_scaling_factor = config.param("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(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(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(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(resolution); voxelmap->insert(*subsampled_submaps[i]); submaps[i]->voxelmaps.push_back(voxelmap); diff --git a/src/glim/odometry/odometry_estimation_gpu.cpp b/src/glim/odometry/odometry_estimation_gpu.cpp index 6b4cb1e..223e4e3 100644 --- a/src/glim/odometry/odometry_estimation_gpu.cpp +++ b/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("odometry_estimation", "voxel_resolution", 0.5); + voxel_resolution_max = config.param("odometry_estimation", "voxel_resolution_max", voxel_resolution); + voxel_resolution_dmin = config.param("odometry_estimation", "voxel_resolution_dmin", 4.0); + voxel_resolution_dmax = config.param("odometry_estimation", "voxel_resolution_dmax", 12.0); + voxelmap_levels = config.param("odometry_estimation", "voxelmap_levels", 2); voxelmap_scaling_factor = config.param("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(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(resolution, 8192 * 2, 10, 1e-3, *stream); voxelmap->insert(*new_frame->frame); new_frame->voxelmaps.push_back(voxelmap);