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

Improves loop closure scan matcher refinement.
2017-06-30 09:30:09 +02:00 · 2017-06-30 09:30:09 +02:00 · f765e55ea9
parent 955e190166
commit f765e55ea9
7 changed files with 73 additions and 12 deletions
--- 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());
--- 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;
 }
--- 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.,
--- 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<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{
--- 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<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(
--- 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,
--- 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.