From ac693f3e0425d0e019a084a8e801d2082b4270d7 Mon Sep 17 00:00:00 2001 From: zjwoody Date: Mon, 21 Aug 2017 18:01:23 +0200 Subject: [PATCH] Use separate voxel filter options for 3D loop closure. (#465) --- .../proto/constraint_builder_options.proto | 3 ++- .../sparse_pose_graph/constraint_builder.cc | 26 ++++++++----------- .../sparse_pose_graph/constraint_builder.h | 3 ++- docs/source/configuration.rst | 3 ++- 4 files changed, 17 insertions(+), 18 deletions(-) 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 4a5d007..8e293ad 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto @@ -30,6 +30,7 @@ message ConstraintBuilderOptions { // Threshold for poses to be considered near a submap. optional double max_constraint_distance = 2; + // This is only used for 2D. // Voxel filter used to compute a sparser point cloud for matching. optional sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 3; @@ -60,7 +61,7 @@ message ConstraintBuilderOptions { optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d = 10; - // Voxel filter used for high resolution, 3D loop closure refinement. + // Voxel filter used for high resolution, only used for 3D loop closure. optional sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options = 15; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 5fe33f3..d526240 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -44,7 +44,10 @@ ConstraintBuilder::ConstraintBuilder( : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), - adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), + high_resolution_adaptive_voxel_filter_( + options.high_resolution_adaptive_voxel_filter_options()), + low_resolution_adaptive_voxel_filter_( + options.low_resolution_adaptive_voxel_filter_options()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder::~ConstraintBuilder() { @@ -175,11 +178,10 @@ void ConstraintBuilder::ComputeConstraint( const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); - const sensor::PointCloud filtered_point_cloud = - adaptive_voxel_filter_.Filter(point_cloud); - + const sensor::PointCloud high_resolution_point_cloud = + high_resolution_adaptive_voxel_filter_.Filter(point_cloud); // The 'constraint_transform' (submap i <- scan j) is computed from: - // - a 'filtered_point_cloud' in scan j and + // - a 'high_resolution_point_cloud' in scan j and // - the initial guess 'initial_pose' (submap i <- scan j). float score = 0.f; transform::Rigid3d pose_estimate; @@ -191,7 +193,7 @@ void ConstraintBuilder::ComputeConstraint( // 3. Refine. if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( - initial_pose.rotation(), filtered_point_cloud, point_cloud, + initial_pose.rotation(), high_resolution_point_cloud, point_cloud, options_.global_localization_min_score(), &score, &pose_estimate, &rotational_score)) { CHECK_GT(score, options_.global_localization_min_score()); @@ -204,7 +206,7 @@ void ConstraintBuilder::ComputeConstraint( } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( - initial_pose, filtered_point_cloud, point_cloud, + initial_pose, high_resolution_point_cloud, point_cloud, options_.min_score(), &score, &pose_estimate, &rotational_score)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); @@ -221,14 +223,8 @@ 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); + low_resolution_adaptive_voxel_filter_.Filter(point_cloud); ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; @@ -248,7 +244,7 @@ void ConstraintBuilder::ComputeConstraint( if (options_.log_matches()) { std::ostringstream info; - info << "Node " << node_id << " with " << filtered_point_cloud.size() + info << "Node " << node_id << " with " << high_resolution_point_cloud.size() << " points on submap " << submap_id << std::fixed; if (match_full_submap) { info << " matches"; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 1d6fa2d..81dc868 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -183,7 +183,8 @@ class ConstraintBuilder { submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; - const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; + const sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter_; + const sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; // Histograms of scan matcher scores. diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 7ce6192..a9c997d 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -103,6 +103,7 @@ double max_constraint_distance Threshold for poses to be considered near a submap. cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options + This is only used for 2D. Voxel filter used to compute a sparser point cloud for matching. double min_score @@ -133,7 +134,7 @@ cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fa Not yet documented. cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options - Voxel filter used for high resolution, 3D loop closure refinement. + Voxel filter used for high resolution, only used for 3D loop closure. cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options Voxel filter used for low resolution, 3D loop closure refinement.