Use multi-res scan matching for loop closures too. (#374)

Improves loop closure scan matcher refinement.

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());

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;
 }

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.,

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<mapping::TrajectoryNode>& submap_nodes,
-    const HybridGrid* const submap, const std::function<void()> work_item) {
+    const Submap* const submap, const std::function<void()> 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<mapping::TrajectoryNode>& submap_nodes,
-    const HybridGrid* const submap) {
+    const Submap* const submap) {
   auto submap_scan_matcher =
       common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
-          *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<void()>& 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{

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<scan_matching::FastCorrelativeScanMatcher>
         fast_correlative_scan_matcher;
   };
@@ -120,14 +121,14 @@ class ConstraintBuilder {
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       const mapping::SubmapId& submap_id,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const HybridGrid* submap, std::function<void()> work_item)
+      const Submap* submap, std::function<void()> 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<mapping::TrajectoryNode>& 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(

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,

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.