diff --git a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc index 8a04cef..2a6d72e 100644 --- a/cartographer/mapping/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping/sparse_pose_graph/constraint_builder.cc @@ -55,6 +55,16 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( parameter_dictionary ->GetDictionary("fast_correlative_scan_matcher_3d") .get()); + *options.mutable_high_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("high_resolution_adaptive_voxel_filter") + .get()); + *options.mutable_low_resolution_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary + ->GetDictionary("low_resolution_adaptive_voxel_filter") + .get()); *options.mutable_ceres_scan_matcher_options_3d() = mapping_3d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); diff --git a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto index b10beb9..4a5d007 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -59,6 +59,15 @@ message ConstraintBuilderOptions { ceres_scan_matcher_options = 11; optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d = 10; + + // Voxel filter used for high resolution, 3D loop closure refinement. + optional sensor.proto.AdaptiveVoxelFilterOptions + high_resolution_adaptive_voxel_filter_options = 15; + + // Voxel filter used for low resolution, 3D loop closure refinement. + optional sensor.proto.AdaptiveVoxelFilterOptions + low_resolution_adaptive_voxel_filter_options = 16; + optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d = 12; } diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 820484d..77a9763 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -104,6 +104,16 @@ class SparsePoseGraphTest : public ::testing::Test { linear_z_search_window = 4., angular_search_window = 0.1, }, + high_resolution_adaptive_voxel_filter = { + max_length = 2., + min_num_points = 150, + max_range = 15., + }, + low_resolution_adaptive_voxel_filter = { + max_length = 4., + min_num_points = 200, + max_range = 60., + }, ceres_scan_matcher_3d = { occupied_space_weight_0 = 20., translation_weight = 10., diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index e366412..e995676 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -72,7 +72,7 @@ void ConstraintBuilder::MaybeAddConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(), + submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ @@ -96,7 +96,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(), + submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) { ComputeConstraint( submap_id, submap, node_id, true, /* match_full_submap */ @@ -126,7 +126,7 @@ void ConstraintBuilder::WhenDone( void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const HybridGrid* const submap, const std::function work_item) { + const Submap* const submap, const std::function work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); @@ -143,13 +143,15 @@ void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( void ConstraintBuilder::ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const HybridGrid* const submap) { + const Submap* const submap) { auto submap_scan_matcher = common::make_unique( - *submap, submap_nodes, + submap->high_resolution_hybrid_grid(), submap_nodes, options_.fast_correlative_scan_matcher_options_3d()); common::MutexLocker locker(&mutex_); - submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)}; + submap_scan_matchers_[submap_id] = {&submap->high_resolution_hybrid_grid(), + &submap->low_resolution_hybrid_grid(), + std::move(submap_scan_matcher)}; for (const std::function& work_item : submap_queued_work_items_[submap_id]) { thread_pool_->Schedule(work_item); @@ -219,11 +221,23 @@ void ConstraintBuilder::ComputeConstraint( // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original // CSM estimate. + sensor::AdaptiveVoxelFilter adaptive_voxel_filter( + options_.high_resolution_adaptive_voxel_filter_options()); + const sensor::PointCloud high_resolution_point_cloud = + adaptive_voxel_filter.Filter(point_cloud); + sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( + options_.low_resolution_adaptive_voxel_filter_options()); + const sensor::PointCloud low_resolution_point_cloud = + low_resolution_adaptive_voxel_filter.Filter(point_cloud); + ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; ceres_scan_matcher_.Match( pose_estimate, pose_estimate, - {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, + {{&high_resolution_point_cloud, + submap_scan_matcher->high_resolution_hybrid_grid}, + {&low_resolution_point_cloud, + submap_scan_matcher->low_resolution_hybrid_grid}}, &constraint_transform, &unused_summary); constraint->reset(new OptimizationProblem::Constraint{ diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 91916f3..ab3d060 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -110,7 +110,8 @@ class ConstraintBuilder { private: struct SubmapScanMatcher { - const HybridGrid* hybrid_grid; + const HybridGrid* high_resolution_hybrid_grid; + const HybridGrid* low_resolution_hybrid_grid; std::unique_ptr fast_correlative_scan_matcher; }; @@ -120,14 +121,14 @@ class ConstraintBuilder { void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const HybridGrid* submap, std::function work_item) + const Submap* submap, std::function work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, - const HybridGrid* submap) EXCLUDES(mutex_); + const Submap* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. const SubmapScanMatcher* GetSubmapScanMatcher( diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index 1b1131e..69697c0 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -47,12 +47,23 @@ SPARSE_POSE_GRAPH = { full_resolution_depth = 3, rotational_histogram_size = 120, min_rotational_score = 0.77, - linear_xy_search_window = 4., + linear_xy_search_window = 5., linear_z_search_window = 1., angular_search_window = math.rad(15.), }, + high_resolution_adaptive_voxel_filter = { + max_length = 2., + min_num_points = 150, + max_range = 15., + }, + low_resolution_adaptive_voxel_filter = { + max_length = 4., + min_num_points = 200, + max_range = 60., + }, ceres_scan_matcher_3d = { - occupied_space_weight_0 = 20., + occupied_space_weight_0 = 5., + occupied_space_weight_1 = 30., translation_weight = 10., rotation_weight = 1., only_optimize_yaw = false, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index a495484..1b2f851 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -149,6 +149,12 @@ cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_m cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d Not yet documented. +cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options + Voxel filter used for high resolution, 3D loop closure refinement. + +cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options + Voxel filter used for low resolution, 3D loop closure refinement. + cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d Not yet documented.